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ABSTRACT 



Analytical equations derived for evaluating linear 
estimators are applied to extended-Kalman filters for 
approximate performance evaluation. Two cases were con- 
sidered, a single known target trajectory and multiple 
target trajectories with given probabilities of occurrence. 
For the multiple-trajectory case, equations are derived for 
the mean and covariance of estimation error in terms of 
the conditional expectations. Two examples are presented 
to compare the use of the analytical equations with Monte- 
Carlo simulation. 
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I. INTRODUCTION 



In state estimation problems one of the tasks is the 
simulation of a filter or estimator after designing it. 

The purpose of the simulation is to determine the perform- 
ance of the filter under actual operating conditions and 
to investigate sensitivity to inaccuracies or approxima- 
tions present in the design assumptions. 

Monte-Carlo simulation is a common simulation algorithm 
which is currently used. An important drawback of the 
Monte-Carlo approach is the large amounts of computer 
time generally required to achieve a reasonable degree of 
accuracy. To reduce the computation time requirements, 
fewer Monte-Carlo runs maybe used with an attendant loss 
of accuracy. Because of these limitations of the Monte- 
Carlo approach, an alternative method, cosisting of a 
set of analytical equations, has been derived and a 
computer algorithm has been established for the evaluation 
of estimation and prediction error statistics for linear 
filters. It is possible to characterize the propagation 
of the means and covariances of estimation error of a 
filter by difference equations. These difference equations 
can easily be solved using relatively small amounts of 
computer time. The only disadvantage of the analytical 
equation approach is that it only applies to linear filters 
with precomputed gain schedules. 
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In practice, one may be confronted by nonlinear 
problems, such as space vehicle re-entry or orbit deter- 
mination problems, or fire-control estimation problems. 

A common approach for nonlinear problems is to use an 
extended Kalman filter and to evaluate its performance 
by Monte-Carlo simulation. 

The main objective of this research is to investigate 
the possibility of approximate evaluation of the perform- 
ance of extended Kalman filters by applying the analytical 
equation approach. In Chapter II of this thesis the ana- 
lytical equations are derived for linear time -invariant 
estimators. In Chapter III, the application of the ana- 
lytical equations to problems with multiple tracks occurring 
with given probabilities is investigated and an example is 
presented. In Chapter IV, a common filtering approach for 
nonlinear systems is summarized. In Chapter V, the appli- 
cation of the analytical equations to extended Kalman filters 
is discussed for both the single and multiple track cases 
by using a nonlinear example. Chapter VI contains simulation 
results from another example. The simulations were performed 
using both the analytical equations and the Monte-Carlo 
algorithm for comparison. 

The computer programs that were used are given in 
Appendix A . 
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II. ANALYTICAL EQUATIONS 



A. ASSUMPTIONS 



In the development of analytical equations the following 
assumptions have been made 1 

1. The true state trajectory X(k) for k=0,l,2,.... 
is known. 

2. The measurement equation is 

Z(k)=h(X(k)) + V(k) (2.1) 

where 




2(k) is the q-dimensional measurement vector at 
time t=kT. 



h is a function (v/hich may be nonlinear) of 



X(k). 



V(k) is the measurement noise vector with the 



assumptions 

(a) . E [ V(k) ] =0 for all k >0. 

(The expected value of the measurement 

noise is zero for all time) 

T1 for k=j 

(b) . E|V(k)V(j)^ =( forallk.j^ 

^ ■' for k;^j 

(the measurement noise is uncorrelated). 

(c) . Since the target trajectory is knovm 

E [ X( j)V(k)'^] =X( j) .E[v?k)j =0 for all k,j>0 

3. The estimator is linear and described by the equation 
i(k/k)=X(k/k-l)+G(k) [ Z(k)-CX(k/k-l)] (2.2) 
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(2.3) 



The prediction equation is 
X(k/k-l)=^ X(k-l/k-l)+Au(k-l) 



where 



X(k/k) is the n-dimensional estimate of X(k) 
given measurements 

Z(0),2(l) Z(k). 

A. 

X(k/k-l) is the predicted value of X(k) given 
measurements 2(0) , 2(1) , . . . ,Z(k-l) . 

£ and C are nxn and qxn matrices, respectively, 
and they are assumed to be known. 

U(k-l) is the m-dimensional deterministic forcing 
vector at time t=(k-l)T. 

A is an nxm known matrix. 

G(k) is the nxm gain matrix. 

The gains may be found by any method. For example, 
optimal estimation gains which minimize the sum of the 
variance of estimation error can be obtained by using the 
Kalman equations: 

G,(k)=P(k/k-l) c'^ [ C P(k/k-l) S'^+R(k) ] "^ (2.4) 

P(k/k)= [l-G(k) c] P(k/k-l) (2.5) 

P(k/k-l)= ^ P(k-l/k-l) ^ + Q(k -1) (2.6) 

with the initial conditions 

P(0/-l)=Po =e|[x(0)-Xo] •[ X(0)-Xof 
X(0/-l)=Xo 

(In this research gains determined by using the 
Kalman equations have been used for simulations.) 
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Where 



P(kA)=E|[t(k/k) - X(k)] . [x(k/k) - j (2.7) 

which is the covariance matrix of theoretical ‘ 



estimation error, 



on error, 

P(kA-l)=E( [ X(k/k-l)-X(k)j •[x(kA-l)-X(k)]'^> (2.8) 

Q(k) is the covariance matrix of the random forcing 
input W(k) . i . e . 



E [W(k) W(j)'^j^ 



fi(k) for k=j 



0 for k/j 



(2.9) 



If the estimation equation (2.2) is initialized with 



the value 

X(0AD = Xq = ^ 2^(0) (2.10) 

it can he shovm that the optimal estimate ^(kA) is unbiased, 



i .e . 

E[x(kA) - X(k)] =0 (2.11) 

With the assumptions given above one can derive 
difference equations for the mean of estimation error, the 
covariance of estimation error, the mean of N-step prediction 
error and the covariance of N-step prediction error. These 
equations are derived for the time-invariant estimator given 
by equations (2.2) and (2.3) in the following section. 



B. DERIVATION OF ANALYTICAL EQUATIONS 

1 . Mean and covariance of estimation error 

The true state of the target at time k is X(k) ; thus, 
using Equations (2.2) and (2.3) the estimation error X(k) at 
time k is 
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X(k) ^ X(k/k) - X(k) 

= £2(k-l/k-l) +4U(k-l) + G(k)|^ Z(k) 

- C ^ X(k-l/k-l) - CAU(k-l)] -X(k) (2.12) 

Substituting Equation (2.1) into (2.12) gives 

Sk) =[l - G(k) C j £ X(k-l/k-l) + G(k) V(k) 

+ G(k) h(X(k)) - X(k) 

[i - S(k) cJa U(k-l) (2.13) 

By defining the deterministic matrices S(k) and D(k) as 

S(k) = [ l - G(k) c] g (2.14) 

D(k) = [l - G(k) cJa U(k-l) + G(k) h (X(k)) 

- X(k) (2.15) 

Equation (2.13) becomes 

X(k) = S(k) X(k-l/k-l) + D(k) + G(k) V(k) (2.l6) 
The mean of estimation error is defined as 

E[x(k)] = E [ S(k) X(k-l/k-l) + D(k) + G(k) V(k)j 
The matrices S(k) and D(k) are deterministic, hence 
E [ G(k) V(k) ] = G(k) e[ V(k)j = 0 
and using properties of the expectation operator gives 
E [ ^(k) ] = S(k) E [ X(k-l/k-l) ] + D(k) 

Defin ing 

/i(k) = E [ x(k)j 

gives 

jUik) = S(k) e[ X(k-l/k-l)] + D(k) (2.17) 
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Equation ( 2 . 16 ) can also be written as 



X(k) = S(k) X(k-l/k-l) + D(k) + G(k) V(k) 

+ [ S(k) X(k-l) - S(k) X(k-l)] 

= £(k)[ X(k-lA-l) - X(k-l)J + D(k) 

+ (^k) V(k) + S(k) X(k-l) 

or 

X(k) = S(k) X(k-l) + D(k) + G(k) V(k) 

+ S(k) X(k-l) (2.18) 

Since X(k-l) is de'terministic and 

E[x(k- 1 )] =/2(k-l) 

Equation (2.1?) can be written as 

ju (k) = S(k)^(k-1) + D(k) + S(k) X(k-r) (2.19) 
Equation (2.19) defines the mean of estimation error at time 
k, in terms of the mean of estimation error at time (k-l) . 

The covariance of estimation error is defined 



as 



where 



P(k/k) = E 
= E 



[x(k) 

[ Y(k) Y(k)'^] 



• [ X(k) -/7(k)j 




( 2 . 20 ) 



Y(k) = X(k) -^(k) 



( 2 . 21 ) 
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One can obtain a difference equation for Y(k) by substi- 
tuting (2.18) and ( 2 . 19 ) into (2.21), that is, 

G(k) V(k)+ 

- S(J^>^k-l) 

= ^(k) [ X(k-1) -^(k-1)] + G(k) V(k) 

= £(k) Y(k-l) + G(k) V(k) (2.22) 

Then 

P(k/k) = E [l(k) Y(k)'^J 

= E [ S(k) Y(k-l) Kk-l)*^ S(k)'^j 
+ E [ S(k) Y(k-l) V(k)^ G(k)'^J 
+ E [(^k) V(k) Kk-l)"^ S(k)'^j 
+ E [ G(k) V(k) V(k)'^ G(k)'^| 

Using properties of the expectation operator yields 
P(k/k) = S(k) e[^ Y(k-l) Kk-l)"^] S(k)'^ 

+ S(k) E[Y(k-l) V(k)'^j G(k)"^ 

+ G(k) E[v(k) Kk-l)*^] ^(k)"^ 

+ ^(k) ^(k) G(k)"^ (2.23) 

To obtain expressions for E |^Y(k-l) V(k)^j and E|^V(k) Y(k-l)'^ 
one can start by deriving an expression for Y(0). The 
definition of Y(0) is 

Y(0) = X(0) -//(O) 

= X(0/0) - X(0) - E [ X(0)] 



Y(k) = S(k) X(k-l) + ^) + 
- £(k)^(k-l) - E^) 
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S ince 



= X(0/0) - X(0) - e[ x(o/o)- X(0)] 
e[x(o)] = X(0) , 

Y(0) = X(0/0) - e[ X(o/o)] 

But 

X(0/0) = X(0/-1) + G(0) [ Z(0) - C ^(0/-l) ] 

= - £(0) C] £(0/-l) + G(0) h(X(0)) 

+ G(0) V(0) 

Substituting (2.25) into (2.24) gives 



Y(0) 



Hov/ever , 



[l. - G(0) Cj X(0/-1) + G(0) h(X(0)) 

+ G(o) v(o) - e|[i - 5 ( 0 ) c] X( 0 /- 1 ) 

+ G(0) h(X(0)) + G(0) V(0) 



E [g(0) V(0)j = G(0) E [V(0)j = 0 
E [g( 0) h(X(0)) ] = G(0) h(X(0)) 



and using properties of the expectation operator gives 
1(0) = [i - G( 0 ) c] X(0/-1) + 

+ G(0) V(0) - [ I - G(0) C j E [ X(0/-1 
- G(^Mr(|'(o)) 

/\ 

But X(0/-1) is a deterministic, known quantity, so 
e[x( 0/-D] = X(0/-1) and 



(2.24) 



(2.25) 



Y(0) = G(0) V(0) 



( 2 . 26 ) 



Using Equation (2,22) 



Y(l) = S(l) Y(0) + G(l) V(l) 

= S(l) G(0) V(0) + G(l) V(l) (2.27) 



and it can be shovm that 



Y(k) 



k-1 



I 



k-j-1 

rr s(k-i) 

i=0 



G(j) V(j) +G(k) V(k) 



( 2 . 28 ) 



Equation (2.28) shows that Y(k) is a linear combination 
of V(k) v/ith coefficients which are known cpnstant matrices, 



i . e . 



Y(k) 




(k) V(0 



(2.29) 



where 



k- I -1 

(k) = ^ S(k-i) G( ^ ) for 1=0,1, 2, 

i=0 

L^(k) = G(k) 



(k-1) 

(2.30) 



From Equation (2.29) it is easily seen that 

E [Y(k-D V(k)'^] = E [ V(k) Y(k-l)'^] = 0 
Because of the assumption that E [v(k) V(j)^j = 0 for kf^j . 
This result reduces Equation (2.23) to 

P(k/k) = S(k) P(k-l/k-l) S(k)'^ 

+ G(k) R(k) G(k)'^ (2.31) 
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Equation (2.31) is Ih® error covariance propogation 
equation which expresses the covariance of estimation 



error at time k in terms of the covariance of estimation 
error at time (k-1) . 

2. Initial conditions 

To use Equations (2.19) and (2.31) needs to know 
Initial condition values. 

The mean of estimation error at time k=0 is 



^ (0) = E [x(0/0) - X(0)] 



= E [ X(0/0)] - X(0) 



Using Equation (2.25) this becomes 



/I ( 0 ) = 




+ G(0) h(X(0)) 



But 



and 



e[ ^(o/-1) ] = X(0/-1) 

e[gj:o) h(X(0))] = G(0) h(x(0)) 

E [g(0) V(0)] = 0 



thus , 



/^(o) = [ 5] i(o/-i) - x(o) 

+ G(0) h(x(0)) 



(2.32) 



Equation (2.32) gives the initial condition for Equation (2.19) • 
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The covariance of estimation error at time k=0 is 

?(o/o) =e[y(o) Y( 0 )^j (2.33) 

Substituting Equation ( 2 . 26 ) into (2.33) gives 
P(0/0) = e[ G(0) V(0) ¥(0)"^ £(0)"^] 

= G(0) R(0) 0(0)"^ (2.35) 



Equation (2.35) gives the initial condition for equation (2.31)* 
3. Mean and covariance of -prediction error 



The one-step prediction is given by 

$(k+lA) = 0i(k/k) +4U(k) 



(2.36) 



and the N-step prediction, based on the estimate X(k/k) is 



N-1 



X (k+N/k) = ^ X(k/k) + t U(k+i) (2.37) 

“P ^ 1^0 ~ 



Defining the deterministic matrix A(N) as 

N-1 

A(N) =Y^ ^"^"^AU(k+i) 



i=0 



(2.38) 



then 

X (k+N/k) = X(k/k) + A(N) 

-p ^ 

The N-step prediction error is 

Xp( k+N/k) = Xp(k+N/k) - X(k+N) 

= ^ X(k/k) + A(N) - X(k+N) 



(2.39) 



(2.40) 
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The mean of the N-step prediction error is defined as 

^(k+N/k) = e[ ^(k+N/k)J 

= e[ ^ X(k/k) + A(N) - X(k+N)J 
= ^ E[x(k/k)] + A(N) - X(k+N) (2.4l) 

But 



^(k) = e[ X(k/k) - X(k) j 
= E [X(k/k)j - X(k) 

and 



E[x(k/k)] =^(k) + X(k) (2.A2) 

Substituting (2.42) into (2.41) gives 

yfip(k+N/k) = /^^k) + X(k) + A(N) - X(k+N) (2.43) 



The covariance of N-step prediction error is 



^(k+N/k) = E|^[^p(k+N/k) -^(k+N/k)] •[^^(k+N/k) 

-^(k+N/k) j 



(2.44) 



Using (2.40) and (2.43) yields 



X^(k+N/k) -/^(k+N/k) = X(k/k) + ^) - X(^N) 



- //UM - f X(k) - 



+ X4k+N) 



= /[ X(k/k) - X(k) -/?(k)] 

= ^ [x'(k/k) -/2(k)] (2.45) 
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which, when substituted into (2.44) gives 



P(k+N/k) =E<^[x(k/k) -/^(k) ] [xtk/k) 
= / P(k/k) [/] 



(2.46) 



Equations (2.43) and (2.46) give the mean and 
covariance of N-step prediction error based on the mean 
and covariance of estimation error at time k. 

In practice, the measurement equations are often 



linear, i.e. 

Z(k) = C^X(k) + V(k) (2.4?) 

Then, in the derivation of the analytical equations, h(X(k)) 
can be replaced with C^X(k). This will change Equation 
(2.15) to 

D(k) =[l -G(k) c][^U(k-l) - X(k)] (2.48) 

and Equation (2.32) to 

/^(O) = [ I - G(0) C ] [ X(0/-1) - X(0)] (2.49) 

4 . Summary of key equations 

(1) . Mean of estimation error 

^(k) = S(k)^(k-1) + D(k) + £(k) X(k-D (2.19) 

(2) . Covariance of estimation error 

Zi^A) = s(k) p(k-iA-i) s(k)'^ 

+ G(k) R(k) G(k)"^ 

~ 
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(2.31) 



where 



S(k) = [l - G(k) oj £ (2.14) 

D(k) = [l - 2,(k) cj4U(k-l) 

+ G(k)h(X(k)) - X(k) ( 2 . 15 ) 

with initial conditions 
^(0)=[l - G(0) C] X(0/-1) - X(0) 

+G(0) h(X(0)) (2.32) 

P(0/0) = G(0) R(0) G(0)^ (2.35) 

(3). Mean and covariance of N-step prediction error 



^p(k+N/k) = ^ X(k) + A(N) 

- X(k+N) 

P(k+N/k) = 0^ P(k/k) r ^ 

As/ I 



where 



(2.43) 

(2.46) 



N-1 

^(N) = ^ gN-i-1 ^u(k+i) (2.38) 

i=0 



If the system is linear and time-varying the 0, ^ 
and C, matrices are time dependent and are denoted "by 0{k) , 

As/ 

^(k) , C(k) . Since 

X(k/k-l) = ^(k-1) X(k-lA-l) +^(k-l) U(k-l) (2.50) 



and 



Z(k) = h(X(k)) + V(k) 



(2.51) 
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it is only necessary to replace the and (^matrices with 
^(k-1) , ^ (k-1) and C(k) in Equations (2.14), (2.43), (2.15), 
(2.46) and (2.38) . 

The analytical equations derived in this chapter 
are applicable to linear filters with precomputed gain 
schedules . 

From Equation (2.31) it is seen that the covariance 
of estimation error is independent of the track, however, the 
mean of estimation error is track dependent. 



\ 
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III. APPLICATION OF THE ANALYTICAL EQUATIONS 
TO THE CASE OF MORE THAN ONE TRACK 



The equations derived in the previous chapter are based 
on one. known +rack. Application of the analytical equations 
using one knovm track has been studied in Reference £" 1_7 
and results have been tabulated. 

If it is desired to evaluate the performance of a filter 
for various tracks, a different track can be used for each 
iteration (ensemble member) in a Monte-Carlo simulation. An 
alternative approach based on the previously derived analytical 
equations is: 

(1) . Apply the analytical equations to calculate the 
mean and covariance of estimation error for each track indi- 
vidually; these are the conditional means and covariances. 

(2) . Calculate the overall mean and covariance using 
the results of (1) and relationships involving the means of 
conditional expectation. 

Assume that there are several tracks to be considered; 

(1) (2) (i) (n) 

X(k) ,X(k) .... X(k) .... X(k) 

where each track has a given probability of occurrence 



, 1 . e . 



Pi.P2'P3* • • »Pi* • *Pn 

(i) 1 

p[ X(k) = X(k) J = Pi 

Pi + P2 + . Pi + . . •+ Pn = 1 



( 3 . 1 ) 
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(i) 



(i) 



The meany4^(k) and the covariance J(k/k) can be calculated 

(i) (i) 

for the i*th track X(k) . Since X (k) has been used for the 



Which is the mean of estimation error given the track X(k) . 
Thus, conditional expectations can be computed for each of 
the tracks . 

To calculate the mean of estimation error, one needs a 
relationship between the conditional means and the mean. 

A. CONDITIONAL EXPECTATION 

The conditional expectation is defined as 2_J7 ; 



calculation of the 



(i) (i) 

(k)^ is the conditional expectation. 




i . e . 




( 3 . 2 ) 

(i) 




oo 



a f^(a / 2=5 j) .da^.. .da^ 



(3.3) 



Where 



A is a continuous random vector. 



B is a discrete random vector . 

f(a / B=b.) is the probability density function of the 
0 



random vector A given B-b.. 
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The expected value of a continuous random vector A is 




a (a) da^ . da^ 



da^ (3.4) 



Equation (3*^) can also be written in terms of the joint 
probability density function of continuous random vectors, i.e. 



fCO +CO jffo 

^ •«««/ ^ f^ ^ ^ F d ) . da^i^ • • • . da^^ . db^ • • • 

-J (3.5) 



db. 



m 



From probability theory the joint probability density function 
can be expressed in terms of conditional density function. 
Since the random vector B is discrete (point conditioning), 



/ B=b^) 






(3.6) 



or 






(3.7) 



Substituting Equation (3*7) into (3.5) and replacing the 
integrals with a summation sign for the random vector B 
(since B is a discrete random vector) gives 




•P 




(3.8) 
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the 



The term inside the brackets in Equation ( 3 . 8 ) is 
conditional expectation given by Equation (3.3). Thus, 
Equation (3.8) takes the final form of 

e[ A ]= ^ e[a /B=bJ • P[B=b.] ( 3 . 9 ) 

j 



Equation (3*9) can be used to dofinc tho iriGan of Gstimation 



error in terms of individual means calculated for each track, 



i.e. 



^(k) = E[x(k)j 



n 

i=l 



(i) 

X(k) / X(k)=X(k) 



^ U) 

/^(k) • 

i=l 



(i) 

P[ X(k)=X(k)] (3.10) 



(3.11) 



where 

( i) 

P. = P[ X(k)=X(k)] 

From Equation (2.31) it is seen that the covariance of 
estimation error is independent of the track for linear 
systems. So it can be calculated once for all tracks. 



B. A LINEAR PROBLEM 

An example of the application of the analytical equations 
to a simplified fire control estimation problem has been 
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presented in /~1_7 . In this section, the same problem is 
considered for three tracks with given probabilities of 
occurrence. The simulation has been done using both the 
analytical equations (program 'EVAL' ) and a Monte-Carlo 
algorithm. 

The problem is a part of a simplified anti-aircraft fire 
control estimation problem in one dimension. The filter is 
a Kalman filter which is derived by solving Equations (2.2) 
through (2.11) subject to the following assumptions. The 
model of target motion is a point mass moving with constant 
velocity. The state equations are 



X(k+1) = ^ X(k) +pW(k) 



(3.12) 



where 




(3.13) 



(3.14) 



T is the time between measurements and equals 1 second. 



The measurement equation is 



2(k) = C X(k) + V(k) 

2 (k) = [l o] X(k) + V(k) ( 3 . 15 ) 
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C 





where 



Xj^(k) is "the range of "the "targef. 

X2(k) is the range rate of the target. 
V(k) is the measurement noise. 

w(k) is a random forcing input. 

It is assumed that 
E [v(k)] = 0 

E [ v(k) = 625 (m) ^ 

= R 

E [w(k)] = 0 

E[w(k)^j = 225 (m/sec^)^ 

= Q 



The filter initial conditions are 

50 X 10^ m 



E 



[x(0)J = 



-600 m/sec 
= ^(0/-l) 



( 3 . 16 ) 

(3.17) 

( 3 . 18 ) 

(3.19) 

(3.20) 

(3.21) 



And the initial covariance matrix is 

10 ^ 0 



P(0/-1) = 



(3.22) 



0 10 ^ 

Using this value for P(0/-1) the gain and covariance 
equations were solved to determine the gain schedule £(k) , 



k= 0,1,2 



31 



In the simulations three tracks (true trajectories of the 



target) v/ere used v/ith initial conditions 



(1) 60 X 10^ m 

X(0) = 

-600 m/sec 

-I 



( 2 ) 

X(0) 



55 X 10^ m 
-600 m/sec 



( 3 ) 70 X 10 ^ m 

X( 0 ) = 

-600 m/sec 



(3.23) 



( 3 . 24 ) 



(3.25) 



The probabilities of occurrence of the tracks are 



?! = p[x(k) 4 U)] =0.3 


(3.26) 


= P[x(k)=x^k0j =0.3 


(3.27) 


r 1 




P3 = PLX(k)=X(k)J = 0.4 


(3.28) 



Figures 3*3 through J .6 show a comparison of the results 
obtained by using the analytical equations and Monte-Carlo 
simulation. Continuous curves represent Monte-Carlo results 
and triangles represent the results obtained using the 
analytical equations. 10,000 Monte-Carlo runs were used 
(3000 runs for the first track, 3000 runs for the second 
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track and 4000 runs for the third track). From Figures 3«3 
through 3*6 it is seen that the two sets of results are very 
close -- especially the covariances of estimation error. 
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FIG. 3*1' Time history of mean position. 
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MEAN VELOGITY (X2) 




FIG. 3.2. Time history of mean velocity. 
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MEAN POSITION 
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36 



-ac4 -ac2 OQQ 0C2 D04 oae 



y 



IffiAN VELOCITY 
I ERROR 

I X 10 ^ m/sec 




FIG. 3*^» Velocity estimation error history. 
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FIG. 3.5. Variance of position estimation error vs, 
time . 
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FIG. 3.6. Variance of velocity estimation error vs. 
time . 
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IV. ESTIMATORS FOR NONLINEAR MODELS 



The extended-Kalman filter is a commonly used approach 
for nonlinear estimation problems. In this chapter, the 
application of extended-Kalman filters to nonlinear models 
is summarized. 

Given a continuous and nonlinear estimation problem, 
in order to apply linear discrete filtering theory the 
problem is first linearized and discretized. 

A . DISCRETIZATION 

Consider a nonlinear continuous system of state and 
observation equations given by 

X(t) = f( X(t) , U(t) , t ) (4.1) 

Z(t) .= h(X(t)) + V(t) (4.2) 

The discrete-time representation of these equations has the 
form 

X(k+1) = a(X(k), U(k),k) + W(k) (4.3) 

Z(k) = C(X(k) ) + V(k) (4.4) 

Equation (4.3) can be obtained from (4.1) by using the 
relationship 
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X(k+1) = X(k) + (At)-f( X(k), U(k),k ) 




+W(k) 



(^. 5 ) 



which is the Taylor series expension of equation (^.1) . 

W(k) is a random input which may also he used to approximate 
the effect of the truncated higher-order terms. 

B. LINEARIZATION 

If there is available a nominal trajectory X(k) and the 
control U(k) is known, one can linearize equations (4.3) and 
(4.4) by expanding them in a taylor series about the nominal 
trajectory X' (k) . For the state equations this gives 



X(k+1) = a( X' (k), U(k),k) +Al 



X(k) -X'(k 



(X'(k) ,U(k) ,k 



+ W(k) + Higher order terms (H.O.T) (4.6) 



Defining the matrix £(k) as 




(4.7) 



and truncating the higher-order terms, Equation (4.6) 



reduces to 



X(k+1) = ^(k) X(k) + a(X'(k) ,y(k) ,k) 



- ^(k) X' (k) + W(k) 
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(4.8) 



Since X' (k) is a known quantity, the second and third terms 
of the right hand side of equation (4.8) are known and 
deterministic. Defining the vector U'(k) as 

U’(k) = a( X'(k),U(k),k ) - ^(k) X'(k) (4.9) 

Equation (4.8) becomes 

X(ktl) = ^(k) X(k) + U*(k) + W(k) (4.10) 

Applying the same procedure to Equation (4.4), the 
linearized form of the observation equation is 

Z(k) = H(k) X(k) + C(X'(k) ) - H(k) X’ (k) + V(k) (4.11) 



where 



H(k) 



dC 



X'(k) 



Defining the vector B(k) as 



B(k) = C( X’(k) ) - H.(k) X'(k) 

yields 



(4.12) 



(4.13) 



Z(k) = H(k) X(k) + B(k) + V(k) (4.14) 

Equations (4.10) and (4.l4) represent a linear time- 
varying model, U*(k) and B(k) represent bias terms resulting 
from the linearization process. The analytical equations can 
be applied to this model by replacing the U(k-l) term in 
( 2 . 15 ) v/ith the U*(k-1) term in Equation (2.37) and defining 
new measurement equations given by 
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Z'(k) = Z(k) - B(k) 



= H(k) X(k) + V(k) 
“* — 



(^. 15 ) 



C. THE EXTENDED-KAIJyiAN FILTER 



Consider a nonlinear discrete system of state and 



observation equations given by 



X(k+1) = f(2L(k) ,k ) + W(k) 



(4.16) 



Z(k) = C( X(k) ) + V(k) 



(4.17) 



In these equations f and C are nonlinear functions of the 
state variables X(k) , W(k) is a random forcing input and V(k) 
is measurement noise with the usual assumptions (an uncor- 
related, zero mean random processes) . 



In order to apply the linear filter equations, Equations 
(4.16) and (4.17) are expanded about the nominal trajectory 
X'(k) if it is available. In practice, it is possible to 
have an idea about the target trajectory for some problems, 
like satellite orbit determination problems, in which case 
one may predict the target trajectory to be very close to the 
true trajectory and thus, be able to define a nominal 
trajectory for the purpose of linearization. In such cases 
the gain schedule can be computed before estimation, but in 



E[w(k) W(j)'^] = Q(k) 8 ^. 
E[v(k) Kj)*^] = R(k) 8 ^. 
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other kinds of problems, it may be impossible to obtain a 
nominal trajectory. An alternative is to linearize the 
problem at each time point about the best estimates of the 
states currently available. In this case the gains can only 
be calculated for each sample as the estimates are available. 
This approach is called the extended-Kalman filter. 

Using an estimate to evaluate the linearization Equation 
(4.l6) gives 

X(k+1) = ^(k) X(k) + W(k) (4.18) 



where 



^(k) 






X(k/k) 



Similarly Equation (4.17) yields 



Z(k) = H(k) X(k) + V(k) 



(4.19) 



(4.20) 



where 



H(k) 



a c 
ax 



X(k/k-l) 



(4.21) 



c((k) and H(k) are obtained as indicated in the linearization 
process discussed earlier and evaluated using the estimate as 
the nominal track. The filter estimation equation is 

X(k/k) = X(k/k-l) + G(k) [ Z(k) - C(X(k/k-l))] (4.22) 

The prediction equation is 

X(k/k-l) = f( X(k-l/k-l), k-1) (4.23) 
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The gains are obtained by using the relationship 



G(k) = P(kA-l) H(k)'^ r H(k) P(k/k-l) HCk)*^ 

+ R(k) j 

where 

the theoretical covariance equation is 

P(k/k) = [i - G(k) H(k)] P(k/k-l) 

The covariance propagation equation is 

P(k/k-l) = j^ik-1) P(k-l/k-l) j2f(k-l)^ + Q(k-l) 

and the initial conditions are 

X(0/-1) = E [x( 0 )J 

= X 

— O 

p(0/-l) = 

= Ej[ x(0) - X^] [x(0) - xj Tj 
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( 4 . 24 ) 

( 4 . 25 ) 

( 4 . 26 ) 

( 4 . 27 ) 



V. APPLICATION OF THE ANALYTICAL EQUATIONS TO 
EXTENDED-KALTMN FILTERS: A RE-ENTRY PROBLEI4 

In this chapter, the approximate evaluation of the 
perfoimiance of an extended-Kalman filter using the analyti- 
cal equations is discussed. Since the analytical equations 
are based on linear estimators with pre-computed gain 
schedules, application of these equations to a problem 
with gains which are not pre-computed is an approximation. 

In an extended-Kalman filter the gains are evaluated on- 
line using the estimates as they are produced. This means 
that the gains will vary from run-to-run even if the track 
is the same. This variation is caused by the differences 
in measurement noise which in turn effect the estimates. To 
use the analytical equation approach to evaluate an extended- 
Kalman filter it is assumed that the linearization v/hich 
results in Equations (4.10) and (4.15) is performed using 
the true track. This leads to a pre-computed gain schedule 
that is used to approximate the extended-Kalman filter gain 
schedule . 

A. THE RE-ENTRY PROBLEM 

In order to compare the results of using the analytical 
equations and Monte-Carlo simulation, a particular problem 
was selected which contains significant nonlinearities in 
both the state and observation equations. This problem 
deals with estimation of the altitude, velocity and constant 
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ballistic coefficient of a vertically falling body. The 
measurements are taken at discrete time intervals of 
1 second by a radar that measures range in the presence 
of (discrete) white gaussian noise. The geometry of the 
problem is illustrated in Figure 5*1« It is assumed that 
the body is falling vertically. 




FIG. 5*1* Geometry of Re-entry problem. 



The following definitions are used: 



X^( t) 

X2(t) 

m 

"d 

A 



P 

H • 
M 

r(t) 



Altitude 

Velocity (downward) 

Mass (constant) 

Drag coefficient (constant) 

Reference area for drag evaluation (constant) 

Mass density of the atmosphere 

Radar altitude 

Horizontal distance 

True range 



It is assumed that the effect of gravity is negligible. 

The equations of motion are 

X^(t) = - X^(t) 

= - x.(t)2 

2 m ^ 

The air density is approximated by the exponential function 

»= (5.2) 

where 

X = 5 X 10“-^ (5.3) 

Defining 

Cr.A (> 

X,(t) = ^ (5.'^) 

2 m 

which is a constant ballistic parameter, the state equations 



become 
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X^(t) = -X2(t) 

X^Ct) = - X^(t) 

X^Ct) = 0 

which are of the form 

X(t) = f( X(t) ) (5.6) 

The measurement r(t) (range) is given by 

r(t) = V + (X^(t) - H )2 (5.7) 

and is observed at discrete instants of time so that the 
observed sequence is 

z(k) = + (X^(k) - H )^ + v(k) (5.8) 

where v(k) is white gaussian noise with zero mean and 
constant variance 

E [ v(k)^] = R 

In this case, the output nonlinearity has the form 

C( X(k) ) = V + (X^(k) - H )^ (5.9) 

It is assumed that 
H = 0 

M = 100,000 ft 
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and the filter initial conditions are 



X(0/-1) = 



X 

— o 



3 X 10^ ft 



2 X 10^ ft/sec 

3 X lO"-^ ft"^ 



The assumed initial covariance matrix is 

.6 



(5.10) 



P(0/-1) = 



10 ' 
' 0 
0 



4x10 

0 



0 0 

6 



0 



10 



-4 



(5.11) 



( 5 . 12 ) 



and the variance of measurement noise is 
R = 10 X 10^ (ft)^ 

The true initial conditions of the falling body are 



X(0) = 



Applying Equation (4.5) to Equation (5.5) i't can be shovm 
that the discretized state equations are 

X(k+1) = a( X(k) ) (5.1^) 



3x10^ 


ft 


4 

2x10 


ft/sec 


1x10”^ 


ft"^ 



X^(k+1) = X^(k) - X^(k) + ^3^^^ 



(5.15) 



X.,(k+1) = X.,(k) - X,(k)^ X.^(k) | X^(k)^ X.^^(k) 



.e-XXi(k) ^ x.,(k)^ X„(k)^ g-2TX^(k) 



(5.16) 
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(5.17) 



X^(k+1) = X^(k) 

Linearizing Equations (5*15) - (5*17) it can be shown that 



the ^(k) matrix is 






^(k) 












dx 


X’(k) 








^^1 


^12 


A13 




= 


^21 


^22 


^23 






A31 


"32 


"33 



where X'(k) is the nominal trajectory and the are 



'^11 


= 1 - ( 1 / 2 ) X 2 *(k)^ X^(k) 


(5.19) 


^12 


= -1 + X^(k) X^(k) e"^^l 


(5.20) 


A13 


= i_ X* (k)2 Q-y^ 
2 


(5.21) 


^21 


= x; (k)2x'(k) +i.y 

^ JS 2 


X3(k)e"^^l 




- 2yX2(k)^ X^(k)^ g-27x^(k) 


(5.22) 


^22 


= 1-2 X-2(k) X^Ck) e'^^1^^^ 






_ -|-y X2(k)^ X^Ck) e'^^1^^^ 






+ 3 (k)^ X^Ck)^ 


(5.23) 
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( 5 . 24 ) 



Ajj = -X’(k)2 iKO') - -i-y e-y^iO') 

+ 2 XgOc)^ X^(k) 



31 


= 0 


(5.25) 


^32 


= 0 


(5.26) 


A 33 


= 1 


(5.27) 



The H(k) matrix is 

a c 



X’(k) 



X^(k) -H 



+ (X^(k) - H)2 



0 



0 



( 5 . 28 ) 



In the simulation of the problem, the true track was generated 
by using the discretized state equations (5-15) through 
(5.17). The linearization is made about the track in the 
application of the analytical equations and U*(k) is defined 
as 



u'(k) = a( X'(k),k ) - £(k) x’(k) 



(5.29) 
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If the true track is generated by solving the state 



equations then 




X'(k+ 1 ) = a( X'(k) ,k ) 


( 5 . 30 ) 


and 




u'(k) = x'(k+l) - £(k) x'(k) 


(5.31) 


y'(k-l) = x'(k) - £(k-l) X’(k-l) 


(5.32) 



If one uses a track other than that generated by solving the 
state equations, then Equation (5*29) must be used for 
U (k) . The matrices £(k) , H(k) and the vector utk- 1 ) can 
be used for application of the analytical equations. In the 
Monte-Carlo simulation of the extended-Kalman filter, one 
must replace X (k) with the estimates X(k/k) in Equations 
(5*19) through (5*27) and by X(k/k-l) in Equation (5«28) 

( 0{k) must be evaluated about ^(k/k) and H(k) must be 
evaluated about X(k/k-l) ) . 

Figures 5*5 through 5*10 illustrate the results of the 
analytical equations and the Monte-Carlo simulation of the 
extended-Kalman filter. The continuous curves represent 
the Monte-Carlo results (1000 runs) and the triangles 
represent results from the analytical equations. The 
figures show that the analytical equations have predicted 
better performance than that predicted by the Monte-Carlo 
simulation. Actually, this observation is not generally 
true. The difference between results of the two methods 
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FIG. 5*2. Altitude history of re-entry vehicle. 
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FIG. 5* 3- Velocity history -of re-entry vehicle. 
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FIG. Time history of hallistic coefficient. 
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MEAN ALTITUDE 




FIG. 5.5. Time history of altitude estimation error. 



K-SCRLr:--5.00r+00 UNITS INCH. 
--SCRLF.--I^GOe>Gl UNITS INCH. 
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FIG. 5«6. Time history of the velocity estimation error. 

X-SCRLr.---'5.00r+00 units inch. 

V'-5CRLL---i.00r.-t01 UNITS INCH- 
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FIG. 5.7. Time history of the mean ballistic parameter 
estimation error. 
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FIG. 5.8. Variance of altitude estimation error vs. 
time . 
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FIG. 5*9 • Variance of velocity estimation error vs. 
time . 
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FIG. 5.10. Variance of ballistic parameter estimation 
error vs. time. 
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depends on the measurement noise, the nature of the nonlinear- 
ities and the filter Initial conditions. For the re-entry 
problem the two results are very close. For example, at 
50,000 ft altitude, the mean altitude error difference 
between the two results is 30 feet. 

In order to investigate the noise dependence of the 
difference between two results, the problem has been solved 
by each method for various covariances of the measurement 
noise and the root mean square of the difference has been 
calculated. The root mean square of the difference between 
two results is defined as 




where 

S^(k), VAR j^(k) are the mean and variance of estimation 

error calculated by the Monte-Carlo 
simulation , 

^o(k), VAR (k) are the mean and variance of estimation 

error calculated by the analytical 
equations , 

N is the number of time points, and 
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^ (I) are the rms values of the 

difference between the two 
results . 

The results obtained have been tabulated in Table 5.1. 

From, Table 5«1 it is seen that the root mean square 
differences betv/een the two results for both altitude and 
velocity estimation error increase as the covariance of 
measurement noise increases. But this is not true for 

which is the ballistic coefficient. Table 5-1 indicates 
that estimation of the ballistic coefficient is relatively 
insensitive to the measurement noise level. 

Figures 5 *11 through 5-16 represent the results of 
50 Monte-Carlo runs (continuous curves) and the analytical 
equations. It is seen that there is significant difference 
between 1000 Monte-Carlo run-results and 50 Monte-Carlo 
run-results. (See Figure 5-5-5*10). 
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FIG. 5' 11* Time history of the mean altitude estimation 
error for 50 Monte-Carlo runs. 



K-SCRLF.---5.00f.+00 UNITS INCH. 
V-SCRLF.-2.00f +01 UNITS INCH. 
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FIG. 5-13* Time history of the mean ballistic 
parameter estimation error for 
50 Monte-Carlo runs. 

X-3CnLE---5.00E+00 UNITS INCH. 
V-SCRLF.-5.00E-04 UNITS INCH. 
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FIG. Variance of the altitude estimation error 

vs. time for 50 Monte-Carlo runs. 

K-SCRL[-5.00E'+00 UNITS INCH. 

V-SCRLF-S.OOf.+Oa UNITS INCH. 
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FIG. 5 ‘ 15 - Variance of the velocity estimation error 
vs. time for 50 Monte-Carlo runs 

><-SCRLE-5.00E+00 UNITS INCH. 

V-SCRLE---1.00E+04 UNITS INCH. 
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FIG. 5*16. Variance of the ballistic parameter estimation 
error vs. time for 50 Monte-Carlo runs. 

^-SCRL[^-5.00E+00 UNITS INCH. 

V-SCfiLF.--5.00E-06 UNITS INCH. 
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Next, the problem was solved for a different target 
track with initial conditions given by 



3.5 


X 


10-^ 


ft. 






2 


X 


10^ 


ft/sec 


= ^ 


(5.35) 


1 


X 


1 

o 
1 — 1 


1/ft 







The results are illustrated in Figures 51? through 5-22. 

One can see that the analytical equations predict worse 
performance than that predicted by the Monte-Carlo simulation. 
For comparison, the CPU times used for both method are 



given below. 

Monte-Carlo simulation (1000 runs) 3 21 sec 

Analytical equations 0 min 9 sec 
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MEAN ALTITUDE 




FIG 5.17. Time history of altitude estimation error 
for X( 0 ) = h 

^-5CRLr.-5.00[>00 UHITj INCH* 

'^'-5CRLF.-l»00r+02 UNITS IHCH^ 
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FIG. 5.18. Time history of mean velocity estimation 
error for X(0)=b. 
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Time history of the ballistic parameter 
estimation error for ^ 



/-SCflLF.---'5.00E+00 UNITS INCH. 
Y-SCni.E=1.00E-03 UNITS INCH. 
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FIG. 5*20. Variance of altitude estimation error 
vs. time for X(0)=b. 

X-SC.RLr.-S.OOE+OO UNITS INCH. 

'"'-SCHLfS^S.OOC + OS UNITS INCH- 
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FIG. 5.21. Variance of velocity estimation error 
vs. time for X(0)=b. 

rS-SCRLF-S.OOE+OO UNITS INCH. 
y-SCRLF.-5.00E+0S UNITS INCH. 



77 



000 005 010 Ql5 020 02S 



VARIANCE OF BALLISTIC 




K-SCRLe>-.5.G0e>00 UNITS INCH. 
V-SGRLC-S.OOf.-OC UNITS INCH. 



78 



B. APPLICATION OF THE ANALYTICAL APPROACH TO THE MULTIPLE 
TRACK NONLINEAR CASE 



The results derived in Chapter III can also be applied 
to nonlinear problems. Equation (3.11) can be used to 
calculate the mean of estimation error in terms of the 
conditional means of estimation error calculated for each 
track. The conditional means are computed approximately by 
using the analytical equations. 

Since the matrices ^(k) and H(k) are calculated using 
the true target trajectory in the nonlinear case, one can 
see that the covariance of estimation error will be dependent 
on the tracks (see Equation (2.31)* Thus, one needs a 
relationship to compute the covariance of estimation error 
in terms of the conditional covariances of estimation error. 



i . e . 



(1) (2) (i) (n) 

P(k/k) = f( P(k/k), P(k/k) P(k/k),.., P(k/k) ) 

(5-36) 



where P(k/k) is the conditional covariance of estimation 

(i) 

error corresponding to the i'th track X(k) . The conditional 
covariance of estimation error is defined as 



(i) (i) 

P(k/k) = Covar X(k)/X(k)=X( k) 

The relationship of the covariance to the conditional 
covariances is derived in the following paragraphs. 



(5.37) 
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C. CONDITIONAL COVARIANCE 



Given a discrete random vector B, the expected value of 
the dependent continuous random vector A can he expressed 
in terms of the conditional expectations as 



E[A] Ej^VB=bJ • P[B=bj] 



(5-38) 



For simplicity Equation (5«38) can be written as 



E [ A ] = E E [ VB ] 



B 



(5.39) 



where the subscript B denotes the expectation with respect 
to the random vector ^ 

The conditional covariance is defined as [^J 

iT 



^A/B ® 



A - e[^b]| • A - e[^b]| 



I 



/Bj (5.40) 



But 



Pa = E 



|[ A - E I A 1 ] • [a - E {Al] 



T 



(5.41) 



which is the (unconditional) covariance matrix. Replacing 



A with 



[a - E|A|j * [a - E I A|j in Equation (5. 39) gives *-2t- 

= E E I [a . E j A 1 ] • [a - EjAl]'^ / b| 



B (5.42) 



**Equation (5.39) can be written in the general case as 
E{g(a)} = E{ E[g(a)/A] } where g(a) may be any function. 
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By adding and subtracting E [Vb] inside the parentheses, 
Equation (5*^2) can be written as 



Pa = El E 



[a - e[a/b]) + [e[a/b] - e[a]) 

A - e[^b] j + |e[vb] - e[a] / b 



B 



or 



■A = E 



:| e| (a - e[vb]) • (A - e[vb])^ 

+ (a - e[Vb] ) • (e[Vb] - e[a])'^ +(e[Vb] - e[aJ) 

• (a e[vb] )'^ + (e[vb] - e[a]) 

• (e[Vb] - e[a])’’| / b| 3 



( 5 .^ 3 ) 



Using the properties of the expectation operator in the 
inner expectation, the cross terms can be calculated as 

E I (A - e[Vb] ) • Ce[Vb] - e[a])Vb|= e{(A -e[^b])/ b| 

*(e[vb] - e[a])'^ 

(because e|^A j and E^^b] are not functions of A, they are 
deterministic with respect to A for a given value of B. 

This means E^ | e|^^B j |= E^A j and E^| E^^b| /B | = E^|^b| ) . 

Using the properties of the expectation operator yields 
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( 5 .^ 5 ) 



e| (a - e[vi])/b|= e[a/b] - e| e[vb]/b| 

= (e[a/b] - e[a/b]) 

= 0 

Thus, the cross terms drop out in Equation (5.43). Since 
(e [^b] - e[a]) is not a function of A but it is a function 
of B, then Equation (5*43) reduces to 

Pa = e| e ( (a - e[a/b]). (a - e[vb])Vb} 

+ (e[^b] - e[a]) • (e[Vb] - e[a])'' 



'B 



(5.46) 



or 



P. = E 

-^A 



e| (a - e[v4) • (i. - e(^b] )Vb) 



+ E 



Ce[a/b] - e[a]) • (e(a/b] - e[a]; 



B 



T 



B 



(5.4?) 



The inner expectation in the first term of the right hand 
side defines the conditional covariance (see Equation (5-4o), 
hence 

^A = ^{Vb } b ^ ^ 

(5.48) 



Equation (5.48) gives the required relationship between the 
covariance and conditional covariances, i.e. 
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P(k/k) = e \ Covar I X(k)/X(k)=X(k)| J 

’ ’X(k) 

) , (i) (i) m / 

, + E ( ;U(k) -jU(k) ) • ( i/(k)- jU(k) 

: ' ’x(k) 

} 1 (i) 

= E P(k/k) + E ( P(k) - fJ(k) ) 

( lx(k) / 

(i) . ml 

• ( (k) - ju (k) 

>X(k) 

which can also be expressed as 

P(k/k) P(k/k)-p. + ^[^k)-y(k)] 

i=l iSl 

• [yfk) - y (k)] 'p^ (5.49) 

where 

Ji) (i) 

U (k) and ^(k/k) are the conditional mean and 

covariance of estimation error for 

(i) 

the i’th track X(k) , 

M(k) and P(k/k) are the overall mean and covariance 

of estimation error and 

p. is the probability of occurrence of 

^ ( i) 

the i’th track X(k) . 

First, using Equation (3 *11) one can compute the mean of 
estimation error jW(k) . Then using Equation (5*^9) the 
covariance can be calculated. 
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The re-entry problem was simulated for two tracks 



where 



Pi = 



P {X(k)=X(k)f 



= 0.5 



Po = 



< ( 2 ) , 

P {X(k)=X(k)l 



= 0.5 



(5.50) 



(5.51) 



with 



( 1 ) 

x(o)= 



( 2 ) 

X(0) = 



3 x,10^ 


ft. 


4 

2 X 10^ 


ft/ sec 


1 X 10"^ 


1/ft 


3.5 X 10^ 


ft. 



(5.52) 



(5.53) 



2 X 10 ft/sec 

1 X 10"^ 1/ft 

Figures 5-23 through 5*28 illustrate the results. The 
continuous curves represent the Monte-Carlo simulation and the 
triangles represent the results from the analytical equations. 
From the figures one can see that the 1000-member ensemble 
Monte-Carlo simulation predicts better performance. Actually 
the differences between the two results are small compared 
to the values of the state variables. For example, the 
maximum difference between the two results is I50 feet for 
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the altitude error at 3 ^ 0,000 feet altitude (which is the 



initial difference; at later times the two results are 
closer) . The CPU times used for these simulations are 
given below. 

CPU Time 

Monte-Carlo simulation (1000 runs) 3 niin 50 sec 

Monte-Carlo simulation (50 runs) 0 min 17 sec 

Analytical equations 0 min 7 sec 
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5.23. Time history of the mean altitude estimation 
error for two tracks. 



X-SCRLf.-.S.OOf'+OO UNITS INCH. 
V-SCRLiS-5.00E>01 UNITS INCH- 
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FIG. 5 -24. Time history of the mean velocity estimation 
error for two tracks . 



x-scnLr-5.oo[>oo units inch. 
y-SCf)LE.--1.00E;+02 UNITS INCH. 
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FIG. 5- 25* Time history of the mean 
estimation error for two 
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FIG. 5.26. Time history of the variance of 
estimation error for two tracks 



x-scfiLe:---5.ocb'too units inch. 
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VARIANCE OF VELOCITY 




X-SCRLE----5.00E+00 UNITS INCH. 
y-SCRLE-2.00E+0T UNITS INCH. 
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VARIANCE OF BALLISTIC 




FIG. 5.28. Time history of the variance of the 
■ballistic parameter estimation error 
for two tracks. 

^-SCfiLK=-S.00E+00 UNITS INCH. 
V-SCRL05.00E-06 UNITS INCH. 
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VI. A SUBr/IARINE TRACKING PROBLEM 



This chapter discusses an example with linear state 
equations but nonlinear measurement equations. It is a 
problem in which estimates are made of the states of a 
submerged target: heading, velocity, rest frequency, range 

to the target at the closest point of approach ( cpa) and 
distance to cpa. One passive sensor (sono bouy) provides 
measurements of target heading and frequency £~ 4_7 . The 
states are 

Rcpa - Range to the target at the closest point 
of approach from the sensor. 

X : X distance to cpa from the sensor (negative 

cpa ^ 

before cpa, positive after cpa) . 

V : Target speed, 

s 

Q : Target heading. 

S 

F : Target rest frequency. 

o 

It is assumed that the target has constant velocity and 
heading. Figure 6.1 illustrates the geometry of the 
problem. The state equations are /”4j7 




X (k+1) = X 
cpa cpa 



(k) + , Ja . k ) 

s ^s 

(k) + V -T + gp ( Yv ' 

^ ^ s "s 



Vg(k+l) = Vg(k) + g^( ) 



( 6 . 1 ) 



s 
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TARGET 



( 6 . 2 ) 



^(k+1) = e^(k) + g^c ) 

s 

F^(k+1) = F^(k) + g,( 7i ) 
with through g^ the random forcing terms, hence, 

M(k) = s^yg ,7y , y* , k ) 

s s o 

The quantities » Yy ' Tf are random changes in heading, 

''s s o 

velocity and rest frequency. They are assumed to be independent, 
zero mean, piecewise-constant rates of change. They have 
variances defined by 

s s 




The values for the standard deviations were taken from 
typical maneuvering parameters for the target 

I* = 100° / 1000 sec = 1.7^533 x 10~^ rad/sec 

= 10 kts / 1000 sec = 5* 5555 x lo”^ yds/sec^ ( 6 . 3 ) 
s 

= 0.5 Hz/ 1000 sec = 0.5 X lo"^ Hz/sec 
o 

With the expressions for the random forcing terms included, 
the state equation become /~ 4_/ 
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= '*opaO'> ^ • >'cpa<''> ' ^6 ‘ ^ 

s 

W'*"> = ’'opa^'^’ '^s<'^) -T -^-r • ^V 



T^ - A-R_(k) • y^ • T 



cpa 



Vg(k+i) = Vg(k) + y^ • T 

s 



Xk+i) = ^(k) + y^ * T 



( 6 . 4 ) 



F^(k+1) = F^(k) + y^ • T 

o 



where A is +1 for counter-clockwise rotation about the sensor 
and -1 for clockwise rotation about the sensor. A is needed 
to give the correct sign for a given geometry. 



The angle measurement equation is 



0(k) = e^(k) - Tan 

o 



-1 



A-R (k) 
cpa 



= 0 (k) - Tan 

o 



-1 



X (k) 
cpa 

A-R (k) 
cpa' ' 






+ (k) for X ^0 

e cpa 

( 6 . 5 ) 

-180° + (k) for X^p^> 0 



The frequency observation equation is 

F„(k) • 



f(k) = 



P 



+ V^(k) 



( 6 . 6 ) 



V 



R (k)^ + X^^^(k)^ 
cpa cpa 
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where V' (k), V_(k) are measurement noises and V is the 
0 1 P 

velocity of sound in the medium (V is assumed to he l64o yds/ 

P 

sec) . The excitation matrix Q(k) can be found as /~4_7 
Q(k) = E|W(k) •W(k)'*^ j 



Q(k) - 



X .T?Ia 
cpa I 



-R.„ . X . 0 I A. X • T?Ia ^ I 0 



cpa cpa ^0^ 



1-5 cpa 



>r 



'cpa ' 0g 
— — 






i_ _ 



SYMMETRIC 



- - 



2r. 2 



Tfl^ 



-h- - — 



_ _ -I _ 

I 0 

I 



o 



(6.7) 



The first two terms involve state related terms. 
The state transition matrix is 




0 0 0 0 

1 T 0 0 

0 10 0 

0 0 10 

0 0 0 1 



( 6 . 8 ) 






96 



and the linearized measurement matrix is 



H(k) =- 



ax 



X’(k) 



H(k) = 






^0(k) aeoc) 
an ax 

cpa cpa 


ao(k) ae(k) 
S''s 3 9s 


c)0(k) 






af(k) af(k) 
aR ax 

cpa cpa 


af(k) af(k) 

3''s &93 


bf(lr) 
^^0 , 




(6.9) 


A • W’') 


Ia 

( 






R'(k)2 


I 

1 

f 


R’(k)3 






f'(k)2-v^(k) 

p;(k).Vp 


1 

R'(k)3 i 

1 


_ f(k)?v^(k) 

F;(k)’Vp 


R* (k) 
cpa 

R'(k)^ 


1 


1 


I 

_L . 
1 


0 

f'(k) 


f' ( k) -V 

0^ ' p 


R'(k) 1 

/ 


0 


1 

) 


p;(k) 



( 6 . 10 ) 



where x'(k) represents the known track if the analytical 
equations are used and the predicted states (X(k/k-l) ) in 
the actual extended-Kalman filter, f (k) is the predicted 
frequency measurement in the extended-Kalman filter and the 
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true frequency measurement in the analytical equations 
R (k) is defined as 



R’(k) 




+ X’ 
cpa 



(k) 



2 



and the covariance of measurement noise matrix is 




7.61543X 10"^ 



1.6 X 10 



-3 



The filter was simulated using the analytical equations 
the Monte-Carlo simulation with the initial conditions 
below. The time between measurements was assumed to be 
100 seconds. 



E(0/-1) 



3.15 X 10 ^ yds. 
-2.61 X 10 ^ yds. 
3.38 yds/sec 

5.30213 Rad. 

500.432 Hz. 



( 6 . 11 ) 



( 6 . 12 ) 



and 

given 



(6.13) 
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The P(o/-l) matrix is 



P(0/-1)= 



1.6245x10'^ 0 0 

0 4.057x10^ 0 

0 0 2.853 

00 0 

000 



0 0 

0 0 

0 0 

0.596 0 

0 0 . 684 



( 6 . 14 ) 



and the true target initial conditions are 

2992.59 Yds. 

-5295.85 Yds. 

^(0) = 4.504 Yds/sec. 

5.06145 Rad. 

500 Hz. 



(6.15) 



These initial conditions have been obtained from a method 
given in /~4_7.) 

The analytical equations predict the mean of estimation 
error to be oscillatory. Monte-Carlo simulation results have 
indicated that the filter was unstable . The estimation 
error becomes unbounded as the target passes through CPA. 

The reason is that the system has two angle measurement 
equations used depending on the sign of X (see Equation 
(6.5) ). Since the filter has significant estimation error 
up to CPA, the filter "crosses" the CPA at a different 
measurement time than the track does. Thus, at a certain 
instant the filter uses a different angle prediction equation 
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then the actual measurement equation. This results in 

a large residual in the estimation equation and the filter 

begins to diverge. If the filter has estimates with very- 

small error before CPA, the estimator and the target would 

"cross” the CPA in the same sampling interval. This is a 

very difficult task because the geometry of the system 

indicates that any fluctuations in the target heading largely 

affect the R and X 

cpa cpa 

In order to force the sign of ^ (k/k-1) to change in 

Cp 3, 

the same sampling interval as the track, frequency measure- 
ments were tested to determine whether they were "up doppler" 

A 

or "down doppler", then the sign of the X (k/k-1) was reversed 
accordingly so that X and (k/k-1) have the same sign. 

Cp3 Cp3 

The results of this approach are illustrated in Figures 6.2 
through 6.11 for 1000 Monte-Carlo runs shown as the continuous 
curves. The triangles represent the results obtained from 
the analytical equations. 

Comparing the results of the two methods, one can see 
that they are completely different (especially the covariances). 
There are several possible reasons for this, including: 

(1) The extended-Kalman filter uses the estimates and 
predictions for calculation of the ^(k) , ^(k) and ^(k) 
matrices, whereas the true target track was used in the 
analytical equations. Thus, it is reasonable that the gain 
schedules will differ largely if the estimates are poor. 
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(2) Since the linearized measurement matrix j{(k) has 
been used, the analytical equations do not include the time 
sharing angle measurement equations; as far as the analytical 
equations are concerned there is only one angle measurement 
equation, because the two equations yield the same derivative. 

( 3 ) Finally, the analytical equations do not "see" the 
change that has been made in the filter algorithm to force 
the filter to cross the CPA in the same measurement interval 
with the target in the Monte-Carlo simulation. 
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FIG. 6.6. Time history of the mean estimation error. 
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VII. CONCLUSIONS 



It has been shown that the analytical equations can be 
used to evaluate the performance of a linear filter with a 
pre-computed gain schedule. The results obtained from the 
analytical equations are exact and are only closely approxi- 
mated by Monte-Carlo simulations with large ensemble sizes 
and correspondingly large CPU times. 

For the case of the extended-Kalman filter, it is 
possible to obtain analytical results which approximate 
those obtained by Monte-Carlo simulations. How close the 
results are, depends on the performance of the filter. The 
main difference between the two methods is that one uses the 
true track whereas the other uses the estimates and predictions 
as the nominal trajectory to evaluate the matrices ^(k), 

H(k) (and g(k),R(k) in some cases, too). If the filter 
performs poorly and gives diverging estimates, one can 
expect that the matrices ^(k) , H(k), Q(k) and £(k) will be 
completely different in the two methods. This will cause 
different results in the two methods. 

The matrix Q(k) effects only the gain schedule. If it is 
a constant matrix or independent of the states, there will be 
no difference between results obtained by the analytical 
equations and Monte-Carlo simulation. The matrix ^(k) has 
two kinds of effects. One is similar to that which g(k) has, 
the other one is that the R(k) matrix represents the covariance 



112 



of measurement noise, and in Chapter V, it has been shovm 
that the "larger" ^(k) is the, larger the difference betv/een 
results becomes. From the results given in Chapter V, it 
is observed that the initial filter states effect the results 
too. Depending on how close the initial filter states are 
to the true initial states, the time at which the filter 
converges v/ill change. 

In summary, one can use the analytical equations to 
approximately predict the performance of an extended-Kalman 
filter. The accuracy of prediction depends on the nature of 
the problem and the filter. 
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APPENDIX A 



COMPUTER PROGRAMS 

A. PROGRAM "EVAL" 

The Program "EYAL" is a computer algorithm which solves 
the analytical equations derived in Chapter II. The algorithm 
has been written for the special case of the analytical 
equations which is developed for the . 86 Fire Control 
System as discussed in Reference • The estimation 

equation has been assumed as 

^(k/k) = X(k/k-l) + G(k) [ M^(k) Z(k) + M^(k) 2(k-l) 

+ M^Ck) Z(k-2) - C X(k/k-l)] (A.l) 

and the measurement equation as 

Z(k) = H X(k) + V(k) (A. 2) 



where H is the true measurement matrix C is the measurement 
matrix with synthetic measurements. The matrices JM^(k) , 
M,(k), M^(k) are measurement weighting matrices which have 
been used in the MK . 86 Fire Control System. 

In the development of this research these matrices are 



defined as 

M (k) = I (qxq Identity matrix) 

M^(k) = = £ 



(A. 3) 
(A.^) 
(A. 5) 
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Then Equation (A.l) reduces to the simpler form 

X(k/k) = X(k/k-l) + G(k)[ Z(k) - C X(k/k-l)] (A. 6) 

With the assumptions given above and starting with the 
definition of the estimation error, i.e. 

X(k) = t(kA) - X(k) (A. 7) 

(where X(k) is the true estimation error vector) 
the following analytical results have been derived in 

1. Covariance of estimation error 

P(kA) = f’(k-l/k-l) Sj(k)* + S^(k) R(k) S^(k)'^ 

+S,(k) R(k-l) S,(k)'’' + S,(k) R(k-2) S,(k)''' 

+ A,(k)'^ + A_(k) + A.(k)'^ (A. 8) 

2. Mean of estimation error 

M (k) = S^(k) U (k-1) + D(k) + S^(k) X(k-l) (A. 9) 

3. Initializing values 

P(0/0) = S^(0) R(0) (A. 10) 

jU ( 0 ) = [i - G(o) c] x(o/-i) - [l - 2io) 

(A. 11) 

4. Mean of N-step prediction error 

jUp (N+k/k) = ^jU(k) + f X(k) - X(N+k) (A. 12) 
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5* Covajrianc6 of N-sfep prediction error 
P(N+k/k) + ^ P(k/k) [ gNj 

where 

So(k) = G(k) M^(k) 

S^(k) = G(k) 115^(k) 

SgCk) = G(k) 

Sj(k) =[l - G(k) c]0 

ii<k) = SjCk) s^(k-l) R(k-1) Sj^(k)^ 

A2(k) = S3(k) [S3(k-1) S^(k-2) + S^(k-l) ] R(k-1) SjCk)'^ 

=[So<''> S - i] >i X(k-l) + SjCk) H X{k-1) 

With the assumptions given by Equations (A. 3 ) - (A. 5 ), 

Equations (A. 8 ) through (A. 13 ) reduce to the form of 

equations derived in Chapter II. 

The computer program given in /” 1_7 has been used after 
making some small changes and adding three subroutines. The 
program listing is given at the end of this Appendix. In 
order to use the program, one must provide subroutine AK 
which computes the linearized state transition matrix ^(k) , 
subroutine H(k) which calculates the matrix HKK and 
subroutine UK which calculates the vector U (k-l) . If the 
matrices ^(k) and R^(k) are required to be calculated on-line, 
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then subroutines QON and RON must also be provided. It is 
also necessary to give the proper values to certain flags 
for required operation. 

B. MONTE-CARLO SIMULATION 

A listing of the program used for Monte-Carlo simulation 
is also given at the end of this Appendix. The program has 
been set up for use with both linear and extended-Kalman 
filters. One can simulate a linear* or extended-Kalman 
filter by selecting the proper flags and supplying the proper 
subroutines. For extended-Kalman filter simulation, one 
must provide subroutine MEAS for simulation of measurements, 
subroutine TRACK for generating the target track (if not 
read in), subroutine HKK for calculation of H(k) , subroutine 
AK for calculation of ^(k) , subroutine CK for calculation 
of C(X(k/k-l) ), subroutine XPRED for calculation of 
X(k+l/k) = f ( X(k/k) ), and subroutines QON and RON for 
Q(k) and R(k) if they are not read in. 
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DIMENSION A( 9 , 9 ) ,B( 9 , 9 ) ,C( 9 , 9 ) 
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3S1KM1 (9,9i ,RK( 9, 9 ) , S0KM2 ( 9 . 9 ) , RKMl (9 , 9 ) » RKM 2 ( 9 , 9 ) , S3KM1 ( 9,91 t 
4TMPl(9,9) ,TMP2(9,9) , PK ( 9 , 9 ) , XK < 9 ) , VTM PI ( 9 ) , XH IT( 9 ) , VTM P2 ( 9 ) , 
5C0M(9) , VTMP3( 9) ,MUK(9) ,XKM1(9) ,XKM2(9) , 0 K ( 9 J , N, L , NA , K, NO , NSAM 
6 IP, IPl , IH, 10, PH<(9,9 ,60) , HK( 9, 9,60) ,QK( 9,9,60) , PMUKS 1 ( 9 , 60 ) , 
7PMUKS2(9,60) ,PKS1 ( 9 , 9 , 60 ) , PK S2 ( 9 , 9 , 60 ) 
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39 YP(K)=MUKS( I fK) 

WRITE (6»85) 

CALL FLOTP ( XP, Y P, NSAM, 0 ) 
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THIS SUBROUTINE COMPUTES LINEARIZED MEASUREMENT MATRIX H(K)=HK(I,J 
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DO 502 K=1,NS4M 
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